#include "aimbot.h"
#include "../autoaim_node.hpp"
#include <cmath>
#include <iterator>

double getYawResolution(const Target &target, const bool isBigArmor)
{
    double dis = sqrt(pow(target.x, 2) + pow(target.y, 2) + pow(target.z, 2));
    double half_armor_width = 0;
    if (6 == autoaim_node->mode)
    {
        half_armor_width = 38;
    }
    else if (isBigArmor)
    {
        half_armor_width = half_big_armor_width;
    }
    else
    {
        half_armor_width = half_small_armor_width;
    }

    return atan2(half_armor_width, (dis * 1000.0)) * 180 / Util::PI;
}

double getPitchResolution(const Target &target)
{
    double dis = sqrt(pow(target.z, 2) + pow(target.xy_plane_distance, 2));

    double half_armor_height = 0;
    if (6 == autoaim_node->mode)
    {
        half_armor_height = 38;
    }
    else
    {
        half_armor_height = 27.5;
    }
    double angle = atan2(half_armor_height, (dis * 1000));
    return (angle * 180 / Util::PI);
}

AimBot::AimBot()
    : if_next_frame_track(false)
#ifdef _USE_NETWORK
      ,
      armor_detector("../src/aimbot/armor_detector/params/model.xml", "CPU",
                     {
                         {InferenceEngine::PluginConfigParams::KEY_CPU_THREADS_NUM, "3"},
                     })
#endif
{
    this->curr_state = this->next_state = AimState::SEARCH_STATE;
}

AimBot::~AimBot() = default;

void AimBot::init()
{
    loadParam();
    roi_rect = cv::Rect();
    armors.reserve(30); // 因为armor比较大，为避免拷贝开销
}

void AimBot::loadParam()
{
    // 相机内参矩阵参数fy，用于小孔成像测距
    camera_fy = g_camera_internal_matrix.at<double>(1, 1);

#ifdef DISTORTION_CORRECT
    camera_internal_matrix = file_storage["camera_internal_matrix"];
    distortion_coeff = file_storage["distortion_coeff"];
#endif // DISTORTION_CORRECT

#ifndef COMPILE_WITH_CUDA
    // 取合适的核大小
    kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(KERNEL_SIZE, KERNEL_SIZE));
#else
    kernel = cv::cuda::createMorphologyFilter(MORPH_CLOSE, CV_8U, getStructuringElement(MORPH_RECT, Size(KERNEL_SIZE, KERNEL_SIZE)));
#endif
}

// flag_switch_armor 是一个非常严格的条件， 一旦为真，预测程序会直接删掉历史点
// 因此，需要在漏识别/装甲板闪灭的情况下， flag_switch_armor 为假
// 换言之，
// 只有在search模式下搜索到的装甲板与历史装甲板像素距离过大的时候直接扔掉 或者是
// 反小陀螺算法下
// 追踪模式实现追踪同一个机器人，装甲板切换以flag_switch_armor为标志

void AimBot::run(std::vector<Armor> armors, ReadPack work_msg, ArmorDir dir, const Pose &pose, double yaw_speed)
{
    int time = pose.mcu_time;

    onEnter();
    // TODO: what propose for canvas?
    if (work_msg.isRightMouseButtonClicked && std::abs(last_click_right - time) > 200) // 若操作手点击了右键
    {
        // DEBUG(
        //     std::cout<<"click right!"<<std::endl;
        // );
        // RCLCPP_INFO(autoaim_node->get_logger(), "click right!");
        last_click_right = time;
        this->curr_state = AimState::SEARCH_STATE;
    }

    if (curr_state == AimState::SEARCH_STATE) // search
    {
        targetSearched = searchArmor(armors, work_msg);
        autoaim_node->autoaim_recorder.target_searched = targetSearched;
        autoaim_node->autoaim_recorder.is_searching = 1;
    }
    else // track
    {
        autoaim_node->autoaim_recorder.is_searching = 0;
        trackArmor(armors, work_msg, dir, pose, yaw_speed);
        if (flag_track_car_failed) // 在车辆都track失败的情况下重新search，几乎为进入search的唯一条件
        {
            if_next_frame_track = false;
        }
        else
        {
            if_next_frame_track = true;
        }
        autoaim_node->autoaim_recorder.flag_track_car_failed = flag_track_car_failed;
    }
    // DEBUG(drawArmors(canvas, armors); drawBars(canvas, lightbars); displayInfo(canvas););
    onExit();
}

bool AimBot::searchArmor(std::vector<Armor> armors, ReadPack work_msg)
{

    //    std::cout << "in serach mode" << std::endl;
    // gradeArmors(armors);                                                 /* 对匹配得到的装甲板们进行误差分数赋值 */
    // sortArmors(armors);                                                  /* 根据误差分数降序排列 */
    trySearchArmor(armors, work_msg.enemy_number, work_msg.enemy_color); /* 数字识别过滤误识别的装甲板并排序 */
    if (!armors.empty())
    {
        // std::cout <<"armor!empty" << std::endl;
        flag_switch_armor = true;
        armor_lost_cnt = 0;
        car_lost_cnt = 0;
        target_armor = armors.at(0);
        curr_number = target_armor.classifier_num;
        is_balance_infantry = 0;
        big_armor_cnt = 0;

        if (ROI_ENABLE)
        {
            setRoiRect(target_armor.rect()); // 把框出装甲板的旋转矩形的包围矩形适当放大
        }
        armorFound = true;
    }
    else
    {
        flag_switch_armor = false;
        roi_rect = cv::Rect();
        armorFound = false;
        is_balance_infantry = 0;
    }
    // response->point.x = roi_rect.x;
    // response->point.y = roi_rect.y;
    // response->height = roi_rect.height;
    // response->width = roi_rect.width;
    // response->flag_if_search = true;
    return armorFound;
}

void AimBot::setRoiRect(const cv::Rect &roiRect)
{
    int detect_x, detect_y;
    int detect_width, detect_height;

    detect_width = static_cast<int>(roiRect.height * 12.0);    //  宽 * 3
    detect_height = static_cast<int>(roiRect.height * 10.0);   //  高 * 4
    detect_x = roiRect.x - (detect_width - roiRect.width) / 2; // x成员， 左上角
    detect_y = roiRect.y - (detect_height - roiRect.height) / 2;

    preventROIExceed(detect_x, detect_y, detect_width, detect_height);
    // roi_rect = cv::Rect(0,0,1280,960);
    roi_rect = cv::Rect(detect_x, detect_y, detect_width, detect_height); // 左上角， 宽度、 高度
    // RCLCPP_INFO(autoaim_node->get_logger(), "%d, %d, %d, %d", detect_x, detect_y, detect_width, detect_height);
}

// void AimBot::preventROIExceed(int &x, int &y, int &width, int &height) {
//     if (x < 0) x = 0;
//     if (y < 0) y = 0;
//     if (x + width > canvas.cols) width = canvas.cols - x;
//     if (y + height > canvas.rows) height = canvas.rows - y;
// }
void AimBot::preventROIExceed(int &x, int &y, int &width, int &height)
{
    if (x < 0)
        x = 0;
    if (y < 0)
        y = 0;
    if (x + width > this->width)
        width = this->width - x;
    if (y + height > this->height)
        height = this->height - y;
}

void AimBot::trackArmor(std::vector<Armor> armors, ReadPack work_msg, ArmorDir dir, const Pose &pose, double yaw_speed)
{
    // std::cout << "in track mode" << std::endl;
    try_track_new(armors, dir, work_msg.enemy_color, yaw_speed); /* 追踪装甲板 */
    if (armorFound)
    {
        if (ROI_ENABLE)
        {
            setRoiRect(target_armor.rect()); // 把框出装甲板的旋转矩形的包围矩形适当放大
        }
    }
    else
    {
        roi_rect = cv::Rect();
    }
    // response->point.x = roi_rect.x;
    // response->point.y = roi_rect.y;
    // response->height = roi_rect.height;
    // response->width = roi_rect.width;
    // response->flag_if_search = false;
}

void AimBot::config()
{
    this->curr_state = this->next_state = AimState::SEARCH_STATE;
}

void AimBot::onEnter()
{
    this->curr_state = this->next_state;
    lightbars.clear();
    armors.clear();
}

void AimBot::onExit()
{
    if (armorFound)
    {
        armor_type_recorder.push(target_armor.isBigArmor ? 1 : 0);
        judge_armor_type(armor_type_recorder, target_armor.isBigArmor);
        if (6 != autoaim_node->mode)
        {
            if (target_armor.isBigArmor && !is_balance_infantry)
            {
                // RCLCPP_INFO(autoaim_node->get_logger(), "classifier_num: %d", target_armor.classifier_num);
                big_armor_cnt++;
            }
            if (big_armor_cnt >= 5 && ((target_armor.classifier_num == 3) || (target_armor.classifier_num == 4) || (target_armor.classifier_num == 5)))
            {
                big_armor_cnt = 0;
                is_balance_infantry = true;
            }
        }
        autoaim_node->autoaim_recorder.is_big_armor = target_armor.isBigArmor;
        autoaim_node->autoaim_recorder.is_balance_infantry = is_balance_infantry;
    }
    old_armor = target_armor;
    /* 当前帧处于搜索装甲板状态 */
    if (this->curr_state == AimState::SEARCH_STATE)
    {
        if (targetSearched)
        {
            this->next_state = AimState::ARMOR_TRACK_STATE;
        }
        else
        {
            this->next_state = AimState::SEARCH_STATE;
        }
    }
    /* 当前帧处于追踪装甲板状态 */
    else
    {
        if (if_next_frame_track)
        {
            this->next_state = AimState::ARMOR_TRACK_STATE;
        }
        else
        {
            this->next_state = AimState::SEARCH_STATE;
        }
    }
    autoaim_node->autoaim_recorder.next_state = this->next_state;
}

void AimBot::judge_armor_type(const CircleQueue<int, 6> &armor_type_recorder, bool &isBigArmor)
{
    int big_armor_cnt = 0;
    for (int i = armor_type_recorder.head; i != armor_type_recorder.tail; i = (i + 1) % armor_type_recorder.size)
    {
        big_armor_cnt = big_armor_cnt + armor_type_recorder.values[i];
    }
    double ratio = big_armor_cnt / armor_type_recorder.length;
    if (ratio < 0.5)
    {
        isBigArmor = false;
    }
    else
    {
        isBigArmor = true;
    }
}

// void AimBot::drawBars(cv::Mat &canvas, std::vector<Lightbar> &bars)
// {
//     LOGM("灯条数量{}", lightbars.size());
//     for (auto &bar: bars) {
//         cv::circle(canvas, bar.rect.center, 2, cv::Scalar(0, 255, 0), -1, 8, 0);

//         cv::Point2f Points[4];
//         bar.rect.points(Points);
//         for (int i = 0; i < 4; i++)
//         {
//             line(canvas, Points[i], Points[(i+1)%4], cv::Scalar(0, 255, 255), 2);
//         }
//     }
// }

// void AimBot::drawArmors(cv::Mat &canvas, std::vector<Armor> &armors)
// {
//     LOGM("装甲板数量{}", armors.size());
//     if (curr_state == AimState::ARMOR_TRACK_STATE) { Util::drawRotatedRect(canvas, old_armor.armor_rect, cv::Scalar(255, 255, 255)); }
//     if (armors.empty()) return;

//     for (auto &armor: armors)
//     {
//         //// 红色：绘制候选装甲板们
//         Util::drawRotatedRect(canvas, armor.armor_rect, cv::Scalar(0, 0, 255));
//         // putText(canvas, to_string(armor.armor_angle),
//         // armor.armor_rect.center, cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0,255, 0), 1, 8);\
//             // 绘制识别图案
//         cv::putText(canvas, std::to_string(armor.classifier_num), armor.armor_rect.center, cv::FONT_HERSHEY_SIMPLEX, 0.6,
//                     [&]() -> cv::Scalar {
//                         switch (old_armor.armor_color) {
//                             case ArmorColor::BLUE:
//                                 return cv::Scalar(255, 0, 0);
//                             case ArmorColor::RED:
//                                 return cv::Scalar(0, 0, 255);
//                             case ArmorColor::GRAY:
//                             case ArmorColor::ERROR:
//                             default:
//                                 return cv::Scalar(255, 255, 255);
//                         }}());
//     }
//     if (armorFound)
//     {
//         Util::drawRotatedRect(canvas, target_armor.armor_rect, cv::Scalar(0, 255, 0)); /* 绿色: 绘制待打击装甲板 */
//         //TODO:
//         cv::putText(canvas, "Armor Type:" + std::to_string(target_armor.isBigArmor), cv::Point(20, 140), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
//     }
// }

void AimBot::displayInfo(cv::Mat &canvas)
{
    int width = canvas.cols;
    int height = canvas.rows;
    int defult_value = 1; /* 若不传入value时的默认值 */

    /// 自瞄模式
    if (curr_state == AimState::SEARCH_STATE)
    {
        cv::putText(canvas, "Searching!", cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 0, 255));
    }
    else
    {
        cv::putText(canvas, "tracking!", cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }
    /// 切换目标标志
    if (flag_switch_armor)
    {
        putText(canvas, "armor switched!", target_armor.armor_rect.center, cv::FONT_HERSHEY_COMPLEX, 0.6,
                cv::Scalar(0, 255, 0), 1, 8);
    }
}

// 追踪部分，来自老代码的track_armor.cpp

bool judge_if_two_armor_share_same_lightbar(const Armor &left_armor, const Armor &right_armor)
{
    if (std::abs(left_armor.rbar_rect.center.x - right_armor.lbar_rect.center.x) < 0.1)
    {
        return true;
    }
    else
    {
        return false;
    }
}

/**
 * @brief 2023赛季装甲板追踪逻辑判断函数
 * @author 井智
 * @param armors
 * @param dir
 * @param color
 * @param yaw_speed
 * @return * void
 */
void AimBot::try_track_new(std::vector<Armor> &armors, ArmorDir dir, int color, double yaw_speed)
{
    autoaim_node->autoaim_recorder.track_test = 0;
    autoaim_node->autoaim_recorder.failedmode = 0;
    // autoaim_node->autoaim_recorder.armor_num1 = armors.size();
    autoaim_node->autoaim_recorder.dir = (int)dir;
    /*----------1.设定不同对象的不同参数-------------------*/
    // 是否是平衡步兵
    // RCLCPP_INFO(autoaim_node->get_logger(), "进入追踪");

    if (is_balance_infantry)
    {
        // DEBUG(
        //     std::cout<<"is banlance!"<<std::endl;
        // );
        car_max_lost_cnt = 100;
    }
    else if (curr_number == 7)
    {
        car_max_lost_cnt = 100;
    }
    else
    {
        car_max_lost_cnt = 6;
    }
    // DEBUG(
    //         // std::cout<<"icar_lost_cnt!"<<car_lost_cnt<<std::endl;
    //     );
    /*----------2.无效装甲板筛选-------------------*/
    // y进行筛选
    // for (auto iter = armors.begin(); iter != armors.end();)
    // {
    //     if (fabs(iter->armor_rect.center.y - old_armor.armor_rect.center.y) > 2 * old_armor.armor_rect.size.height)
    //     {
    //         iter = armors.erase(iter);
    //     }
    //     else
    //     {
    //         ++iter;
    //     }
    // }

    // 判断是否为空
    autoaim_node->autoaim_recorder.car_lost_cnt = car_lost_cnt;
    autoaim_node->autoaim_recorder.armor_lost_cnt = armor_lost_cnt;

    // autoaim_node->autoaim_recorder.armor_num = 0;
    if (armors.empty())
    {
        flag_switch_armor = false;
        armorFound = false;
        ++armor_lost_cnt;
        if (armor_lost_cnt >= armor_max_lost_cnt)
        {
            ++car_lost_cnt;
            if (car_lost_cnt >= car_max_lost_cnt)
            {
                flag_track_car_failed = true;
                autoaim_node->autoaim_recorder.failedmode = 1;
                return;
            }
            else
            {
                flag_track_car_failed = false;
                return;
            }
        }
        else // 缂佈呯敾鏉╁€熼嚋鐟佸懐鏁抽敓锟�?
        {
            flag_track_car_failed = false;
            return;
        }
    }

    RCLCPP_DEBUG(autoaim_node->get_logger(), "control_mode: %d", autoaim_node->control_mode);
    // RCLCPP_INFO(autoaim_node->get_logger(), "dishit: %d, %d, %d, %d,%d, %d,%d, %d,", autoaim_node->queue_dishit[0], autoaim_node->queue_dishit[1], autoaim_node->queue_dishit[2], autoaim_node->queue_dishit[3], autoaim_node->queue_dishit[4], autoaim_node->queue_dishit[5], autoaim_node->queue_dishit[6], autoaim_node->queue_dishit[7]);
    if (autoaim_node->control_mode) // 哨兵专用
    {
        for (int i = 0; i < armors.size();)
        {
            if (autoaim_node->tb.enemy.empty() || (std::find_if(autoaim_node->tb.enemy.begin(), autoaim_node->tb.enemy.end(), [&](auto &enemy)
                                                                { return (enemy.number == armors.at(i).classifier_num); }) == autoaim_node->tb.enemy.end()))
            {
                cv::Mat trans_mat;
                cv::Mat rotate_mat;
                cv::Point2f left_up = armors.at(i).coord_points[0];
                cv::Point2f left_down = armors.at(i).coord_points[3];
                cv::Point2f right_up = armors.at(i).coord_points[1];
                cv::Point2f right_down = armors.at(i).coord_points[2];
                double half_w = 0, half_h = 0;

                if (armors.at(i).isBigArmor)
                {
                    // std::cout << "is big armor!" << std::endl;
                    half_w = half_big_armor_width;
                    half_h = half_big_armor_height;
                }
                else
                {
                    half_w = half_small_armor_width;
                    half_h = half_small_armor_height;
                }
                std::vector<cv::Point2f> points2d;
                points2d.clear();
                std::vector<cv::Point3f> points3d;
                points3d.clear();

                points2d.emplace_back(left_up);
                points2d.emplace_back(right_up);
                points2d.emplace_back(right_down);
                points2d.emplace_back(left_down);

                points3d.emplace_back(cv::Point3f(-half_w, -half_h, 0));
                points3d.emplace_back(cv::Point3f(half_w, -half_h, 0));
                points3d.emplace_back(cv::Point3f(half_w, half_h, 0));
                points3d.emplace_back(cv::Point3f(-half_w, half_h, 0));

                solvePnP(points3d, points2d, g_camera_internal_matrix, g_distortion_coeff, rotate_mat, trans_mat, false, cv::SOLVEPNP_IPPE);
                armors.at(i).target.x = ((trans_mat.at<double>(0, 0) + X_OFFSET)) / 1000.0;
                armors.at(i).target.y = ((trans_mat.at<double>(1, 0) + Y_OFFSET)) / 1000.0;
                armors.at(i).target.z = ((trans_mat.at<double>(2, 0) + Z_OFFSET)) / 1000.0;

                Eigen::Vector3d t_vec_eigen(armors.at(i).target.x, armors.at(i).target.y, armors.at(i).target.z);

                // 默认roll pitch yaw顺序定义欧拉角
                Eigen::Vector3d euler_angle_a(-autoaim_node->pose.ptz_roll * CV_PI / 180.0, -autoaim_node->pose.ptz_pitch * CV_PI / 180.0, 0);
                Eigen::Matrix3d rotate_mat_world_to_camera;
                rotate_mat_world_to_camera = Eigen::AngleAxisd(euler_angle_a[2], Eigen::Vector3d::UnitZ()) *
                                             Eigen::AngleAxisd(euler_angle_a[1], Eigen::Vector3d::UnitY()) *
                                             Eigen::AngleAxisd(euler_angle_a[0], Eigen::Vector3d::UnitX());

                // 然后绕z轴和x轴分别转-90,得到y轴朝下相机坐标系
                Eigen::Vector3d euler_angle_b(-M_PI_2, 0, -M_PI_2);
                Eigen::Matrix3d rotate_mat_world_to_camera_b;
                rotate_mat_world_to_camera_b = Eigen::AngleAxisd(euler_angle_b[2], Eigen::Vector3d::UnitZ()) *
                                               Eigen::AngleAxisd(euler_angle_b[1], Eigen::Vector3d::UnitY()) *
                                               Eigen::AngleAxisd(euler_angle_b[0], Eigen::Vector3d::UnitX());

                // 然后只需要将两个旋转矩阵按照顺序右乘即可得到相机坐标系到世界坐标系的旋转矩阵。
                rotate_mat_world_to_camera = rotate_mat_world_to_camera * rotate_mat_world_to_camera_b;

                t_vec_eigen = rotate_mat_world_to_camera * t_vec_eigen;
                interfaces::msg::Enemy enemy_msg;
                enemy_msg.x = t_vec_eigen[0];
                enemy_msg.y = t_vec_eigen[1];
                enemy_msg.number = armors.at(i).classifier_num;
                enemy_msg.set__is_aimed(false);
                autoaim_node->tb.enemy.push_back(enemy_msg);
            }

            if (1 == autoaim_node->queue_dishit[armors[i].classifier_num])
            {
                armors.erase(armors.begin() + i);
            }
            else
            {
                ++i;
            }
        } // 第一步：根据屏蔽字筛掉不需要的装甲板

        // tb.time = autoaim_node->time_process;

        int index = 0;
        for (int i = 0; i < autoaim_node->queue_hit_priority.size(); ++i)
        {
            if (curr_number == autoaim_node->queue_hit_priority[i])
            {
                index = i;
                break;
            }
        } // 得到现在追踪装甲板的优先级
        for (int i = 0; i < armors.size(); ++i)
        {
            int tmp_index = 0;
            for (int j = 0; j < autoaim_node->queue_hit_priority.size(); ++j)
            {
                if (armors[i].classifier_num == autoaim_node->queue_hit_priority[j])
                {
                    tmp_index = j;
                    break;
                }
            } // 得到armors[i]的优先级
            if (curr_number < 3 || curr_number > 5 || armors[i].classifier_num < 3 || armors[i].classifier_num > 5) // 只要不是二者都是步兵的情况，即可比较优先级
            {
                if (tmp_index < index) // 如果存在比目前追踪装甲板优先级更高的装甲板
                {
                    flag_track_car_failed = true;
                    RCLCPP_INFO(autoaim_node->get_logger(), "发现优先级更高的装甲板，切换！");
                    return;
                }
            }
        } // 第二步：检查是否存在优先级更高的装甲板
    }

    if (USE_MODEL)
    {
        for (auto iter = armors.begin(); iter != armors.end();)
        {
            bool condition = (iter->classifier_num == curr_number);
            if (!condition)
            {
                iter = armors.erase(iter);
            }
            else
            {
                ++iter;
            }
        }
    }

    /* 閺嶈宓佺憗鍛暢閺夊じ绗屾稉濠佺鐢嶆嫹?閿熺晫鏁抽弶鍨禈閸嶅繐娼楅弽鍥獩缁傚妾锋惔蹇斿笓鎼村骏绱� 鐎圭偟骞囨潻鍊熼嚋閿燂拷?閿燂拷?*/
    /// TODO!!!、
    // 按照距离进行排序
    for (auto &armor : armors)
    {
        cv::Mat trans_mat;
        cv::Mat rotate_mat;
        cv::Point2f left_up = armor.coord_points[0];
        cv::Point2f left_down = armor.coord_points[3];
        cv::Point2f right_up = armor.coord_points[1];
        cv::Point2f right_down = armor.coord_points[2];
        double half_w = 0, half_h = 0;

        if (6 == autoaim_node->mode)
        {
            half_w = 38;
            half_h = 38;
        }
        else if (armor.isBigArmor)
        {
            // std::cout << "is big armor!" << std::endl;
            half_w = half_big_armor_width;
            half_h = half_big_armor_height;
        }
        else
        {
            half_w = half_small_armor_width;
            half_h = half_small_armor_height;
        }
        std::vector<cv::Point2f> points2d;
        points2d.clear();
        std::vector<cv::Point3f> points3d;
        points3d.clear();

        points2d.emplace_back(left_up);
        points2d.emplace_back(right_up);
        points2d.emplace_back(right_down);
        points2d.emplace_back(left_down);

        points3d.emplace_back(cv::Point3f(-half_w, -half_h, 0));
        points3d.emplace_back(cv::Point3f(half_w, -half_h, 0));
        points3d.emplace_back(cv::Point3f(half_w, half_h, 0));
        points3d.emplace_back(cv::Point3f(-half_w, half_h, 0));

        solvePnP(points3d, points2d, g_camera_internal_matrix, g_distortion_coeff, rotate_mat, trans_mat, false, cv::SOLVEPNP_IPPE);
        armor.target.x = ((trans_mat.at<double>(0, 0) + X_OFFSET)) / 1000.0;
        armor.target.y = ((trans_mat.at<double>(1, 0) + Y_OFFSET)) / 1000.0;
        armor.target.z = ((trans_mat.at<double>(2, 0) + Z_OFFSET)) / 1000.0;

        Util::wheel2land(armor.target, autoaim_node->pose);

        armor.diff_position = cv::norm(armor.armor_rect.center - old_armor.armor_rect.center);
        // armor.diff_position = Util::get3Ddistance(armor.target, old_armor.target);
        // RCLCPP_INFO(autoaim_node->get_logger(), "diff_position: %lf", armor.diff_position);
    }
    // RCLCPP_INFO(autoaim_node->get_logger(), " ");
    std::sort(armors.begin(), armors.end(), Armor::sort_diff_position_func);

    // auto tovision_priority_comparator = [](const Armor& a, const Armor& b)
    // {
    //     int index_a = 0, index_b = 0;
    //     for (int i = 0; i < autoaim_node->queue_hit_priority.size(); ++i)
    //     {
    //         if (a.classifier_num == autoaim_node->queue_hit_priority[i])
    //         {
    //             index_a = i;
    //         }
    //         if (b.classifier_num == autoaim_node->queue_hit_priority[i])
    //         {
    //             index_b = i;
    //         }
    //     }
    //     return index_a < index_b;
    // };
    /* 閺佹澘鐡х拠鍡楀焼,娴犲懍缍旀潻鍥ㄦ姢閿燂拷?閿燂拷?*/
    /* 濞夈劍鍓�!!! 閸︺劋绗傛稉鈧敮褝鎷�?閿熺晫鏁抽弶鎸庢殶鐎涙ぞ绗夋稉娲祩閺冭泛鎯庨悽銊ㄦ嫹閿燂拷?閻楃懓鐣鹃弫鏉跨摟 */
    // 按照数字进行筛选一遍

    autoaim_node->autoaim_recorder.armor_num2 = armors.size();

    // 数字筛选之后装甲板依旧剩余大于两个
    if (USE_MODEL && armors.size() > 2)
    {
        armorFound = false;
        flag_switch_armor = false;
        ++armor_lost_cnt;
        if (armor_lost_cnt >= armor_max_lost_cnt)
        {
            ++car_lost_cnt;
            if (car_lost_cnt >= car_max_lost_cnt)
            {
                flag_track_car_failed = true;
                autoaim_node->autoaim_recorder.failedmode = 2;
                return;
            }
            else
            {
                flag_track_car_failed = false;
                return;
            }
        }
        else // 缂佈呯敾鏉╁€熼嚋鐟佸懐鏁抽敓锟�?
        {
            flag_track_car_failed = false;
            return;
        }
    }
    // 当前有效的装甲板为0
    if (armors.size() == 0)
    {
        armorFound = false;
        flag_switch_armor = false;
        ++armor_lost_cnt;
        if (armor_lost_cnt >= armor_max_lost_cnt)
        {
            ++car_lost_cnt;
            if (car_lost_cnt >= car_max_lost_cnt)
            {
                flag_track_car_failed = true;
                autoaim_node->autoaim_recorder.failedmode = 3;
                return;
            }
            else
            {
                flag_track_car_failed = false;
                return;
            }
        }
        else // 缂佈呯敾鏉╁€熼嚋鐟佸懐鏁抽敓锟�?
        {
            flag_track_car_failed = false;
            return;
        }
    }
    autoaim_node->autoaim_recorder.armor_num3 = armors.size();

    for (size_t i = 0; i < autoaim_node->tb.enemy.size(); i++)
    {
        if (autoaim_node->tb.enemy[i].number == armors.at(0).classifier_num)
        {
            // std::cout << "enemy:" << autoaim_node->tb.enemy[i].number << "classify:" << armors.at(0).classifier_num << std::endl;
            autoaim_node->tb.enemy[i].is_aimed = true;
        }
    }
    // 遍历完成后将消息转换完成

    /*----------3.判断是否进行了装甲板切换-------------------*/
    // 一共就四种情况：
    // {一个目标装甲板，两个装甲板}与{已知装甲板切换方向，未知装甲板切换方向}
    // 旋转方向已知
    // RCLCPP_INFO(autoaim_node->get_logger(), "进入第三步");
    // dir = ArmorDir::UNKNOWN;
    autoaim_node->autoaim_recorder.if_two_armor = 0;
    if (dir != ArmorDir::UNKNOWN)
    {
        // RCLCPP_INFO(autoaim_node->get_logger(), "进入有方向模式");
        // 装甲板只有一个
        if (armors.size() == 1)
        {
            // 左转状态
            if (dir == ArmorDir::LEFT)
            {
                autoaim_node->autoaim_recorder.if_two_armor = 1;
                // 判断唯一的装甲板的结果
                // judgesArmorSwitched_antigyro： 0：未切换 -1：误识别到中间 1：装甲板已切换 2：错误装甲板
                int switch_result = judgesArmorSwitched_antigyro(armors.at(0), old_armor, yaw_speed);
                if (switch_result == 1 && (armors.at(0).target.yaw <= old_armor.target.yaw))
                {
                    target_armor = armors.at(0);
                    armor_lost_cnt = 0;
                    car_lost_cnt = 0;
                    flag_track_car_failed = false;
                    flag_switch_armor = true;
                    armorFound = true;
                    autoaim_node->autoaim_recorder.track_test = 1;
                    return;
                }
                else if (switch_result == 0)
                {
                    armorFound = true;
                    target_armor = armors.at(0);
                    flag_track_car_failed = false;
                    flag_switch_armor = false;
                    armor_lost_cnt = 0;
                    car_lost_cnt = 0;
                    autoaim_node->autoaim_recorder.track_test = 3;
                    return;
                }
                else
                {
                    ++armor_lost_cnt;
                    if (armor_lost_cnt >= armor_max_lost_cnt) // 鐡掑懎鍤崗渚婃嫹?閿熷€熸嫹閿燂拷?閸氬奔绔撮崸妤嬫嫹?閿熺晫鏁抽弶璺ㄦ畱婢惰精瑙﹀▎鈩冩殶閻ㄥ嫰妲囬敓锟�?,鐎电粯澹樻潪锔跨瑐閻ㄥ嫬褰熸径鏍︾閸ф鎷�?閿熺晫鏁抽敓锟�?
                    {
                        armorFound = false;
                        flag_switch_armor = false;
                        ++car_lost_cnt;
                        if (car_lost_cnt >= car_max_lost_cnt) // 鏉╁€熼嚋鏉烇箒绶犳径杈Е
                        {
                            autoaim_node->autoaim_recorder.track_test = 4;
                            flag_track_car_failed = true;
                            autoaim_node->autoaim_recorder.failedmode = 4;
                            return;
                        }
                        else
                        {
                            autoaim_node->autoaim_recorder.track_test = 5;
                            flag_track_car_failed = false;
                            return;
                        }
                    }
                    else
                    {
                        flag_track_car_failed = false;
                        flag_switch_armor = false;
                        armorFound = false;
                        autoaim_node->autoaim_recorder.track_test = 6;
                        return;
                    }
                }
            }
            else
            { // 右转状态
                int switch_result = judgesArmorSwitched_antigyro(armors.at(0), old_armor, yaw_speed);
                if (switch_result == 1 && (armors.at(0).target.yaw > old_armor.target.yaw))
                {
                    autoaim_node->autoaim_recorder.if_two_armor = 1;
                    target_armor = armors.at(0);
                    armor_lost_cnt = 0;
                    car_lost_cnt = 0;
                    flag_track_car_failed = false;
                    flag_switch_armor = true;
                    armorFound = true;
                    autoaim_node->autoaim_recorder.track_test = 7;
                    // std::cout<<"切换装甲板"<<std::endl;
                    return;
                }
                else if (switch_result == 0)
                {
                    armorFound = true;
                    target_armor = armors.at(0);
                    flag_track_car_failed = false;
                    flag_switch_armor = false;
                    armor_lost_cnt = 0;
                    car_lost_cnt = 0;
                    autoaim_node->autoaim_recorder.track_test = 9;
                    // std::cout<<"正常"<<std::endl;
                    return;
                }
                else
                {
                    // std::cout<<"错误"<<std::endl;
                    ++armor_lost_cnt;
                    if (armor_lost_cnt >= armor_max_lost_cnt) // 鐡掑懎鍤崗渚婃嫹?閿熷€熸嫹閿燂拷?閸氬奔绔撮崸妤嬫嫹?閿熺晫鏁抽弶璺ㄦ畱婢惰精瑙﹀▎鈩冩殶閻ㄥ嫰妲囬敓锟�?,鐎电粯澹樻潪锔跨瑐閻ㄥ嫬褰熸径鏍︾閸ф鎷�?閿熺晫鏁抽敓锟�?
                    {
                        armorFound = false;
                        flag_switch_armor = false;
                        ++car_lost_cnt;
                        if (car_lost_cnt >= car_max_lost_cnt) // 鏉╁€熼嚋鏉烇箒绶犳径杈Е
                        {
                            flag_track_car_failed = true;
                            autoaim_node->autoaim_recorder.failedmode = 5;
                            autoaim_node->autoaim_recorder.track_test = 10;
                            return;
                        }
                        else
                        {
                            flag_track_car_failed = false;
                            autoaim_node->autoaim_recorder.track_test = 11;
                            return;
                        }
                    }
                    else
                    {
                        flag_track_car_failed = false;
                        flag_switch_armor = false;
                        armorFound = false;
                        autoaim_node->autoaim_recorder.track_test = 12;
                        return;
                    }
                }
            }
        }
        else if (armors.size() == 2)
        {
            int result = 0;
            if (armors.at(0).armor_rect.center.x < armors.at(1).armor_rect.center.x)
            {
                if (dir == ArmorDir::LEFT)
                {
                    target_armor = armors.at(1);
                    target_armor_deputy = armors.at(0);
                    result = 1;
                    autoaim_node->autoaim_recorder.track_test = 15;
                }
                else
                {
                    target_armor = armors.at(0);
                    target_armor_deputy = armors.at(1);
                    result = 0;
                    autoaim_node->autoaim_recorder.track_test = 16;
                }
            }
            else
            {
                if (dir == ArmorDir::LEFT)
                {
                    target_armor = armors.at(0);
                    target_armor_deputy = armors.at(1);
                    result = 0;
                    autoaim_node->autoaim_recorder.track_test = 17;
                }
                else
                {
                    target_armor = armors.at(1);
                    target_armor_deputy = armors.at(0); // change
                    result = 1;
                    autoaim_node->autoaim_recorder.track_test = 18;
                }
            }
            flag_track_car_failed = false;
            int switch_result = judgesArmorSwitched_antigyro(target_armor, old_armor, yaw_speed);
            if (switch_result == 1 && ((dir == ArmorDir::LEFT && (target_armor.target.yaw < old_armor.target.yaw)) || (dir == ArmorDir::RIGHT && (target_armor.target.yaw > old_armor.target.yaw))))
            {
                armor_lost_cnt = 0;
                car_lost_cnt = 0;
                flag_switch_armor = true;
                armorFound = true;
                autoaim_node->autoaim_recorder.track_test = 19;
                return;
            }
            else if (switch_result == 0)
            {
                flag_switch_armor = false;
                armor_lost_cnt = 0;
                car_lost_cnt = 0;
                armorFound = true;
                autoaim_node->autoaim_recorder.track_test = 20;
                return;
            }
            else if (judgesArmorSwitched_antigyro(armors.at((result + 1) % 2), old_armor, yaw_speed) == 0)
            {
                target_armor = armors.at((result + 1) % 2);
                flag_switch_armor = false;
                armor_lost_cnt = 0;
                car_lost_cnt = 0;
                armorFound = true;
                autoaim_node->autoaim_recorder.track_test = 21;
                return;
            }
            else
            {
                flag_switch_armor = false;
                armorFound = false;
                autoaim_node->autoaim_recorder.track_test = 22;
                return;
            }
        }
    }
    else // 无方向模式
    {
        // RCLCPP_INFO(autoaim_node->get_logger(), "进入无方向模式");
        // if(armors.size() == 2){
        if (0)
        {
            RCLCPP_INFO(autoaim_node->get_logger(), "进入无方向模式/双装甲板");
            target_armor = armors.at(0);
            target_armor_deputy = armors.at(1);
            target_armor_deputy.is_empty = false;
            flag_switch_armor = false;
            armorFound = true;
            flag_track_car_failed = false;
            armor_lost_cnt = 0;
            car_lost_cnt = 0;
            // RCLCPP_INFO(autoaim_node->get_logger(), "装甲板赋值完成");
        }
        else
        {
            int switch_result = judgesArmorSwitched_antigyro(armors.at(0), old_armor, yaw_speed);
            // RCLCPP_INFO(autoaim_node->get_logger(), "switch_result=%d", switch_result);
            if (switch_result == 1)
            {
                switch_num++;
                flag_switch_armor = false;
                armorFound = false;
                flag_track_car_failed = false;
                armor_lost_cnt = 0;
                car_lost_cnt = 0;
                autoaim_node->autoaim_recorder.track_test = 23;
            }
            else
            {
                switch_num = 0;
                if (switch_result == 0)
                {
                    flag_switch_armor = false;
                    armorFound = true;
                    flag_track_car_failed = false;
                    armor_lost_cnt = 0;
                    car_lost_cnt = 0;
                    target_armor = armors.at(0);
                    autoaim_node->autoaim_recorder.track_test = 24;
                }
                else
                {
                    flag_switch_armor = false;
                    armorFound = false;
                    flag_track_car_failed = false;
                    armor_lost_cnt = 0;
                    car_lost_cnt = 0;
                    autoaim_node->autoaim_recorder.track_test = 25;
                }
            }

            if (switch_num == 4)
            {
                // RCLCPP_INFO(autoaim_node->get_logger(), "switch num = 4!");
                flag_switch_armor = true;
                armorFound = true;
                flag_track_car_failed = false;
                armor_lost_cnt = 0;
                car_lost_cnt = 0;
                target_armor = armors.at(0);
                autoaim_node->autoaim_recorder.track_test = 26;
            }
        }
    }
}

// 旋转前哨站识别追踪代码
void AimBot::try_track_dynamic(std::vector<Armor> &armors, ArmorDir dir, int color, double yaw_speed)
{
    autoaim_node->autoaim_recorder.track_test = 0;
    autoaim_node->autoaim_recorder.failedmode = 0;
    // autoaim_node->autoaim_recorder.armor_num1 = armors.size();
    autoaim_node->autoaim_recorder.dir = (int)dir;
    // 整体的lost数量
    armor_max_lost_cnt = 300;
    // 允许的前哨站丢帧数量
    car_max_lost_cnt = 4;

    // 两倍的装甲板高度限制
    // for (auto iter = armors.begin(); iter != armors.end();)
    // {
    //     if (fabs(iter->armor_rect.center.y - old_armor.armor_rect.center.y) > 2 * old_armor.armor_rect.size.height)
    //     {
    //         iter = armors.erase(iter);
    //     }
    //     else
    //     {
    //         ++iter;
    //     }
    // }
    autoaim_node->autoaim_recorder.car_lost_cnt = car_lost_cnt;
    autoaim_node->autoaim_recorder.armor_lost_cnt = armor_lost_cnt;
    // 筛选过后没有装甲板存在
    if (armors.empty())
    {
        flag_switch_armor = false;
        armorFound = false;
        ++armor_lost_cnt;
        if (armor_lost_cnt >= armor_max_lost_cnt)
        {
            ++car_lost_cnt;
            if (car_lost_cnt >= car_max_lost_cnt)
            {
                flag_track_car_failed = true;
                return;
            }
            else
            {
                flag_track_car_failed = false;
                return;
            }
        }
        else // 缂佈呯敾鏉╁€熼嚋鐟佸懐鏁抽敓锟�?
        {
            flag_track_car_failed = false;
            return;
        }
    }

    /* 閺嶈宓佺憗鍛暢閺夊じ绗屾稉濠佺鐢嶆嫹?閿熺晫鏁抽弶鍨禈閸嶅繐娼楅弽鍥獩缁傚妾锋惔蹇斿笓鎼村骏绱� 鐎圭偟骞囨潻鍊熼嚋閿燂拷?閿燂拷?*/
    /// TODO!!!
    for (auto &armor : armors)
    {
        armor.diff_position = cv::norm(armor.armor_rect.center - old_armor.armor_rect.center);
    }
    std::sort(armors.begin(), armors.end(), Armor::sort_diff_position_func);

    /* 閺佹澘鐡х拠鍡楀焼,娴犲懍缍旀潻鍥ㄦ姢閿燂拷?閿燂拷?*/
    /* 濞夈劍鍓�!!! 閸︺劋绗傛稉鈧敮褝鎷�?閿熺晫鏁抽弶鎸庢殶鐎涙ぞ绗夋稉娲祩閺冭泛鎯庨悽銊ㄦ嫹閿燂拷?閻楃懓鐣鹃弫鏉跨摟 */
    if (USE_MODEL)
    {
        for (auto iter = armors.begin(); iter != armors.end();)
        {
#ifndef _USE_NETWORK
            // iter->setNumberImg();
            // iter->classifier_num = classify(iter->number_img);
            // // 如果是3,4,5通过分类器判断大小装甲板
            // if (iter->classifier_num == 3 || iter->classifier_num == 4 || iter->classifier_num == 5)
            // {
            //     iter->isBigArmor = bool(classify.type_classify(iter->number_img));
            // }
            // // if (iter->classifier_num == 1 || iter->classifier_num == 6) { iter->isBigArmor = true; }
            // if (iter->classifier_num == 1 || iter->classifier_num == 8 ) { iter->isBigArmor = true; }//2023哨兵
#endif
            bool condition = (iter->classifier_num == curr_number);
            if (!condition)
            {
                iter = armors.erase(iter);
            }
            else
            {
                ++iter;
            }
        }
    }

    autoaim_node->autoaim_recorder.armor_num2 = armors.size();

    if (USE_MODEL && armors.size() > 2)
    {
        armorFound = false;
        flag_switch_armor = false;
        ++armor_lost_cnt;
        if (armor_lost_cnt >= armor_max_lost_cnt)
        {
            ++car_lost_cnt;
            if (car_lost_cnt >= car_max_lost_cnt)
            {
                flag_track_car_failed = true;
                return;
            }
            else
            {
                flag_track_car_failed = false;
                return;
            }
        }
        else // 缂佈呯敾鏉╁€熼嚋鐟佸懐鏁抽敓锟�?
        {
            flag_track_car_failed = false;
            return;
        }
    }

    for (auto &armor : armors)
    {
        cv::Mat trans_mat;
        cv::Mat rotate_mat;
        cv::Point2f left_up = armor.coord_points[0];
        cv::Point2f left_down = armor.coord_points[3];
        cv::Point2f right_up = armor.coord_points[1];
        cv::Point2f right_down = armor.coord_points[2];
        double half_w = 0, half_h = 0;

        if (armor.isBigArmor)
        {
            // std::cout << "is big armor!" << std::endl;
            half_w = half_big_armor_width;
            half_h = half_big_armor_height;
        }
        else
        {
            half_w = half_small_armor_width;
            half_h = half_small_armor_height;
        }
        std::vector<cv::Point2f> points2d;
        points2d.clear();
        std::vector<cv::Point3f> points3d;
        points3d.clear();

        points2d.emplace_back(left_up);
        points2d.emplace_back(right_up);
        points2d.emplace_back(right_down);
        points2d.emplace_back(left_down);

        points3d.emplace_back(cv::Point3f(-half_w, -half_h, 0));
        points3d.emplace_back(cv::Point3f(half_w, -half_h, 0));
        points3d.emplace_back(cv::Point3f(half_w, half_h, 0));
        points3d.emplace_back(cv::Point3f(-half_w, half_h, 0));

        solvePnP(points3d, points2d, g_camera_internal_matrix, g_distortion_coeff, rotate_mat, trans_mat, false, cv::SOLVEPNP_IPPE);
        armor.target.x = ((trans_mat.at<double>(0, 0) + X_OFFSET)) / 1000.0;
        armor.target.y = ((trans_mat.at<double>(1, 0) + Y_OFFSET)) / 1000.0;
        armor.target.z = ((trans_mat.at<double>(2, 0) + Z_OFFSET)) / 1000.0;

        Util::wheel2land(armor.target, autoaim_node->pose);

        armor.diff_position = cv::norm(armor.armor_rect.center - old_armor.armor_rect.center);
        // armor.diff_position = Util::get3Ddistance(armor.target, old_armor.target);
        // RCLCPP_INFO(autoaim_node->get_logger(), "diff_position: %lf", armor.diff_position);
    }
    // RCLCPP_INFO(autoaim_node->get_logger(), " ");
    std::sort(armors.begin(), armors.end(), Armor::sort_diff_position_func);

    if (armors.empty())
    {
        ++armor_lost_cnt;
        if (armor_lost_cnt >= armor_max_lost_cnt) // 鐡掑懎鍤崗渚婃嫹?閿熷€熸嫹閿燂拷?閸氬奔绔撮崸妤嬫嫹?閿熺晫鏁抽弶璺ㄦ畱婢惰精瑙﹀▎鈩冩殶閻ㄥ嫰妲囬敓锟�?,鐎电粯澹樻潪锔跨瑐閻ㄥ嫬褰熸径鏍︾閸ф鎷�?閿熺晫鏁抽敓锟�?
        {
            armorFound = false;
            flag_switch_armor = false;
            ++car_lost_cnt;
            if (car_lost_cnt >= car_max_lost_cnt) // 鏉╁€熼嚋鏉烇箒绶犳径杈Е
            {
                flag_track_car_failed = true;
                return;
            }
            else
            {
                flag_track_car_failed = false;
                return;
            }
        }
        else
        { // 濞屸剝婀佺搾鍛毉閸忎緤鎷�?閿熷€熸嫹閿燂拷?閸氬奔绔撮崸妤嬫嫹?閿熺晫鏁抽弶璺ㄦ畱婢惰精瑙﹀▎鈩冩殶閻ㄥ嫰妲囬敓锟�?
            flag_track_car_failed = false;
            flag_switch_armor = false;
            armorFound = false;
        }
    }
    // 发生装甲板切换判断
    else if (judgesArmorSwitchedDynamic(armors.at(0), old_armor, dir, yaw_speed))
    {
        armorFound = true;
        target_armor = armors.at(0);
        flag_switch_armor = true; //
        flag_track_car_failed = false;
        armor_lost_cnt = 0;
        car_lost_cnt = 0;
        autoaim_node->autoaim_recorder.track_test = 1;
    }
    else
    {
        flag_switch_armor = false;
        armorFound = true;
        target_armor = armors.at(0);
        flag_track_car_failed = false;
        autoaim_node->autoaim_recorder.track_test = 2;
    }
}

bool AimBot::judgesArmorSwitchedDynamic(const Armor &curr_armor, const Armor &old_armor, ArmorDir dir, double yaw_speed)
{
    // 这个根本就用不到因为有可能会出现跨圈的问题
    // double delta_yaw = Util::getYawForPTZ(curr_armor.target) - Util::getYawForPTZ(old_armor.target);
    // 当前的yaw数据和上一次的yaw数据
    double delta_yaw, curr_yaw, last_yaw;
    curr_yaw = Util::getYawForPTZ(curr_armor.target);
    last_yaw = Util::getYawForPTZ(old_armor.target);

    if (curr_yaw - last_yaw > 150)
    {
        last_yaw += 360;
    }
    else if (curr_yaw - last_yaw < -150)
    {
        last_yaw -= 360;
    }
    // delta_yaw_abs = std::abs(curr_yaw - (last_yaw + 0.001 * yaw_speed * (curr_armor.target.clock - old_armor.target.clock)));
    autoaim_node->autoaim_recorder.curr_yaw = curr_yaw;
    autoaim_node->autoaim_recorder.last_yaw = last_yaw;
    // 对于旋转前哨站而言不需要进行yaw数据预测的偏差设置
    delta_yaw = curr_yaw - last_yaw;
    autoaim_node->autoaim_recorder.delta_yaw_abs = curr_yaw - last_yaw;
    // judgeSwitch.csv
    /*--------------------------*/
    // std::ofstream fout;
    // std::string save_addr = "../save/csv/judgeSwitch.csv";
    // fout.open(save_addr, std::ios::app | std::ios::out);
    // if (!fout.is_open()) { std::cout << "unvalid save_addr" << std::endl; }
    /*--------------------------*/
    // 此时并不知道旋转方向

    // RCLCPP_INFO(autoaim_node->get_logger(), "rate: %lf", std::abs(delta_yaw) / Util::getYawResolution(curr_armor.target, curr_armor.isBigArmor));
    bool result = 0;
    if (dir == ArmorDir::UNKNOWN)
    {
        if (std::abs(delta_yaw) > 3 * getYawResolution(curr_armor.target, curr_armor.isBigArmor))
        {
            // fout<<std::endl;
            // fout << Util::getYawResolution(curr_armor.target, curr_armor.isBigArmor)<<","<<curr_yaw << ",";
            // return true;
            result = 1;
            autoaim_node->autoaim_recorder.judge_armor_switch = 1;
        }
        else
        {
            // fout << curr_yaw << ",";
            // return false;

            result = 0;
            autoaim_node->autoaim_recorder.judge_armor_switch = 2;
        }
    }
    else
    {
        // dir为1的时候向右旋转也就是向左切换装甲板,delta为正
        if ((int)dir * delta_yaw > 2.5 * getYawResolution(curr_armor.target, curr_armor.isBigArmor))
        {
            // fout<<std::endl;
            // fout << Util::getYawResolution(curr_armor.target, curr_armor.isBigArmor)<<","<<curr_yaw << ",";
            // return true;
            result = 1;
            autoaim_node->autoaim_recorder.judge_armor_switch = 3;
        }
        else
        {
            // fout << curr_yaw << ",";
            // return false;
            result = 0;
            autoaim_node->autoaim_recorder.judge_armor_switch = 4;
        }
    }
    return result;
}

bool AimBot::judgesArmorSwitched(const Armor &curr_armor, const Armor &old_armor, ArmorDir dir, double yaw_speed)
{
    double delta_yaw = Util::getYawForPTZ(curr_armor.target) - Util::getYawForPTZ(old_armor.target);
    double delta_yaw_abs, curr_yaw, last_yaw;
    curr_yaw = Util::getYawForPTZ(curr_armor.target);
    last_yaw = Util::getYawForPTZ(old_armor.target);
    if (curr_yaw - last_yaw > 150)
    {
        last_yaw += 360;
    }
    else if (curr_yaw - last_yaw < -150)
    {
        last_yaw -= 360;
    }
    delta_yaw_abs = std::abs(curr_yaw - (last_yaw + 0.001 * yaw_speed * (curr_armor.target.clock - old_armor.target.clock)));
    double ratio = 1.0;
    if (delta_yaw_abs > ratio * getYawResolution(curr_armor.target, curr_armor.isBigArmor))
    {
        return true;
    }
    else
    {
        return false;
    }
}

/**
 * @brief 判断一个装甲板是否进行切换
 * @author 井智
 * @param curr_armor 本次判断是否进行装甲板切换的装甲板
 * @param old_armor 用来判断本次装甲板是否切换的上次装甲板
 * @param dir 方向（未知、左、右）
 * @param yaw_speed （转动的速度）
 * @return * int 返回的参数，用来表示不同的情况
 */
int AimBot::judgesArmorSwitched_antigyro(const Armor &curr_armor, const Armor &old_armor, double yaw_speed)
{
    // double delta_yaw = Util::getYawForPTZ(curr_armor.target) - Util::getYawForPTZ(old_armor.target);
    /*1.利用装甲板获取本次、上次的yaw数据*/
    double delta_yaw_abs, curr_yaw, last_yaw;
    curr_yaw = Util::getYawForPTZ(curr_armor.target);
    last_yaw = Util::getYawForPTZ(old_armor.target);

    /*2.跨圈处理*/
    if (curr_yaw - last_yaw > 150)
    {
        last_yaw += 360;
    }
    else if (curr_yaw - last_yaw < -150)
    {
        last_yaw -= 360;
    }
    autoaim_node->autoaim_recorder.curr_yaw = curr_yaw;
    autoaim_node->autoaim_recorder.last_yaw = last_yaw;
    /*3.装甲板记录*/
    // int degree = 2; // 多项式回归的阶数
    // armor_times.push_back(curr_armor.target.clock);
    // armor_yaws.push_back(curr_yaw);
    // double predictedYaw = polynomialRegression(armor_times, armor_yaws, degree, curr_armor.target.clock);
    // if (isArmorSwitched(input_data[i], predictedYaw, threshold)) {
    //     std::cout << "Armor switched at index: " << i << std::endl;
    //     // 重置预测器或执行其他操作
    //     armor_times.clear();
    //     armor_yaws.clear();
    // }
    /*4.利用数据判断是否装甲板切换*/
    // （1）是否使用速度（预测数值对结果进行判断） 否
    // （2）是否对切换装甲板进行多次判断 是
    // （3）切换装甲板分情况判断是用旋转方向已知还是使用第n圈的次数 旋转方向已知
    double ratio2 = 3;
    double ratio1 = 3;
    double ratio3 = 11;
    delta_yaw_abs = std::abs(curr_yaw - (last_yaw + 0.001 * yaw_speed * (curr_armor.target.clock - old_armor.target.clock)));
    autoaim_node->autoaim_recorder.delta_yaw_abs = curr_yaw - last_yaw;

    if (is_balance_infantry)
    {
        ratio1 = 1.5;
        ratio2 = 1.5;
        ratio3 = 15;
    }
    else if (curr_armor.classifier_num == 1)
    {
        ratio1 = 2.8;
        ratio2 = 2.8;
        ratio3 = 11;
    }
    else if (curr_armor.classifier_num == 7)
    {
        delta_yaw_abs = std::abs(curr_yaw - last_yaw);
        ratio1 = 3;
        ratio2 = 3;
        ratio3 = 14;
    }
    /*--------------------------*/
    // std::ofstream fout;
    // std::string save_addr = "judgeSwitchAtiall.csv";
    // fout.open(save_addr, std::ios::app | std::ios::out);
    // // if (!fout.is_open()) { std::cout << "unvalid save_addr" << std::endl; }
    // fout << curr_yaw << "," << delta_yaw_abs / Util::getYawResolution(curr_armor.target, curr_armor.isBigArmor) << "," << Util::getYawResolution(curr_armor.target, curr_armor.isBigArmor) << std::endl;

    // autoaim_node->autoaim_recorder.ratio = (curr_yaw - (last_yaw + 0.001 * yaw_speed * (curr_armor.target.clock - old_armor.target.clock))) / Util::getYawResolution(curr_armor.target, curr_armor.isBigArmor);

    // fout.close();
    /*--------------------------*/

    // geometry_msgs::msg::PoseStamped pose_msg;
    // pose_msg.header.stamp = autoaim_node->now();
    // pose_msg.pose.position.x = curr_yaw;
    // // pose_msg.pose.position.y = P.at<double>(1, 2);
    // // pose_msg.pose.position.z = P.at<double>(2, 2);
    // autoaim_node->pub_pose->publish(pose_msg);

    // 非正常情况-错误装甲板
    int result = 0;
    // if (delta_yaw_abs > ratio3 * getYawResolution(curr_armor.target, curr_armor.isBigArmor))
    // {
    //     // fout<<curr_yaw << ","<<"2"<<",";
    //     // RCLCPP_INFO(autoaim_node->get_logger(), "装甲板错误！");
    //     result = 2;
    //     // return 2;
    // }
    // 正常切换装甲板情况
    if (delta_yaw_abs > ratio2 * getYawResolution(curr_armor.target, curr_armor.isBigArmor))
    {
        // fout<<std::endl;
        // fout<< delta_yaw_abs/Util::getYawResolution(curr_armor.target, curr_armor.isBigArmor)<<","
        // <<delta_yaw_abs<<","<< Util::getYawResolution(curr_armor.target, curr_armor.isBigArmor)<<","
        // <<curr_yaw << ","<<"1"<<",";
        // RCLCPP_INFO(autoaim_node->get_logger(), "装甲板已切换！");
        result = 1;
        // return 1;
    }
    // 误识别中间的情况
    else if (delta_yaw_abs > ratio1 * getYawResolution(curr_armor.target, curr_armor.isBigArmor))
    {
        // fout<<curr_yaw << ","<<"-1"<<",";
        // RCLCPP_INFO(autoaim_node->get_logger(), "装甲板误识别成中间！");
        result = -1;
        // return -1;
    }
    // 正常识别
    else
    {
        // RCLCPP_INFO(autoaim_node->get_logger(), "装甲板没有切换！");
        // fout<<curr_yaw << ","<<"0"<<",";
        result = 0;
        // return 0;
    }
    autoaim_node->autoaim_recorder.judge_armor_switch = result;
    return result;
}

// 搜索部分，来自老代码的grade_armor
static double get_distance_score(double distance)
{
    double score = 0;
    if (distance < 2.0)
        score = 0;
    else if (distance >= 2.0 && distance < 3.0)
        score = distance;
    else if (distance >= 3.0 && distance < 4.0)
        score = 8.0 * (distance - 3) + 3;
    else if (distance >= 4.0 && distance < 5.0)
        score = 12.0 * (distance - 4.0) + 11.0;
    else
        score = 4 * distance;
    return score;
}

static void show_error_score(Armor &armor)
{
    std::cout << "总误差分数: " << armor.sum_error_score << std::endl;
    std::cout << "距离误差分数:" << armor.dis_error_score << "|"
              << "装甲板比例误差分数:" << armor.ratio_error_score << "|"
              << "两灯条高度比误差分数:" << armor.bar_len_ratio_error_score << "|"
              << "两灯条角度差误差分数:" << armor.bar_delta_angle_error_score << "|"
              << "装甲板角度误差分数:" << armor.armor_angle_error_score << "|" << std::endl;
}

void AimBot::gradeArmors(std::vector<Armor> &armors)
{
    if (armors.empty())
        return;

    for (auto &armor : armors)
    {
        // 距离误差分数
        armor.dis_error_score = get_distance_score(armor.distance);
        armor.sum_error_score += armor.dis_error_score;
        // std::cout << "小孔成像距离：" << distance << std::endl;

        // 权重
        if (armor.isBigArmor)
        {
            armor.ratio_error_score = 2 * fabs(armor.ratio - 4.09);
            armor.sum_error_score += armor.ratio_error_score;
        }
        else
        {
            armor.ratio_error_score = 2 * fabs(armor.ratio - 2.363636);
            armor.sum_error_score += armor.ratio_error_score;
        }

        // 两灯条高度比造成的误差分数
        armor.bar_len_ratio_error_score = (armor.bar_len_ratio - 1.0) * 10.0 / (MAX_LENGTH_RATIO - 1.0);
        armor.sum_error_score += armor.bar_len_ratio_error_score;

        // 两灯条倾斜度之差不过大，因为如果是正常的匹配的灯条角度差比较小，权重要稍微高一点
        armor.bar_delta_angle_error_score = (armor.bar_delta_angle / (MAX_LIGHTBAR_DELTA)) * 15.0;
        armor.sum_error_score += armor.bar_delta_angle_error_score;

        // 装甲板倾斜角度误差分数
        armor.armor_angle_error_score = fabs(armor.armor_angle) * 15.0 / MAX_ARMOR_ANGLE;
        armor.sum_error_score += armor.armor_angle_error_score;
        // show_error_score(armor);
    }
}

// search_armor.cpp
static void listArmorsInOrder(std::vector<Armor> &armors)
{
    for (const auto &armor : armors)
    {
        std::cout << "error: " << armor.sum_error_score << "|";
    }
    std::cout << std::endl;
}

bool AimBot::trySearchArmor(std::vector<Armor> &armors, int mcu_number, int mcu_color)
{
    if (armors.empty())
    {
        return false;
    }

    if (USE_MODEL)
    {
        // if (armors.size() >= MAX_CANDIDATE_NUM)// 限制候选装甲板数量ls
        // {
        //     armors.resize(MAX_CANDIDATE_NUM);
        // }
        for (auto iter = armors.begin(); iter != armors.end();)
        {
            // #ifndef _USE_NETWORK
            //             iter->setNumberImg();
            //             iter->classifier_num = classify(iter->number_img);
            //             // if (iter->classifier_num == 1 || iter->classifier_num == 6) { iter->isBigArmor = true; }
            //             // 如果是3,4,5通过分类器判断大小装甲板
            //             if (iter->classifier_num == 3 || iter->classifier_num == 4 || iter->classifier_num == 5)
            //             {
            //                 iter->isBigArmor = bool(classify.type_classify(iter->number_img));
            //             }
            //             if (iter->classifier_num == 1 || iter->classifier_num == 8 ) { iter->isBigArmor = true; }//2023哨兵
            // #endif
            iter->setPriority();
            /* 丢弃误识别装甲板 */
            bool ToDiscard = (iter->classifier_num == 0);
            if (ToDiscard)
            {
                iter = armors.erase(iter);
            }
            else
            {
                ++iter;
            }
        }
    }

    // 对成功识别数字的装甲板进行排序
    // 1. 优先级 2. 误差分数
    if (!armors.empty())
    {
        if (autoaim_node->control_mode)
        {
            // auto tovision_priority_comparator = [](const Armor& a, const Armor& b)
            // {};
            // sort(armors.begin(), armors.end(), );
            for (int i = 0; i < armors.size();)
            {
                if (1 == autoaim_node->queue_dishit[armors[i].classifier_num])
                {
                    armors.erase(armors.begin() + i);
                }
                else
                {
                    ++i;
                }
            } // 第一步：根据屏蔽字筛掉不需要的装甲板

            auto tovision_priority_comparator = [](const Armor &a, const Armor &b) -> bool
            {
                if (a.classifier_num >= 3 && a.classifier_num <= 5 && b.classifier_num >= 3 && b.classifier_num <= 5)
                {
                    return Armor::priorityComparator(a, b);
                }
                else
                {
                    int index_a = 0, index_b = 0;
                    for (int i = 0; i < autoaim_node->queue_hit_priority.size(); ++i)
                    {
                        if (a.classifier_num == autoaim_node->queue_hit_priority[i])
                        {
                            index_a = i;
                        }
                        if (b.classifier_num == autoaim_node->queue_hit_priority[i])
                        {
                            index_b = i;
                        }
                    }
                    return index_a < index_b;
                }
            };
            sort(armors.begin(), armors.end(), tovision_priority_comparator);
            // 第二步：根据哨兵给的优先级排序
        }
        else
        {
            sort(armors.begin(), armors.end(), Armor::priorityComparator);
        }
    }

    if (armors.empty())
    {
        return false;
    }
    else
    {
        return true;
    }
}

void AimBot::sortArmors(std::vector<Armor> &armors)
{
    if (armors.empty())
    {
        return;
    }
    sort(armors.begin(), armors.end(), Armor::scoreComparator);
    // listArmorsInOrder(armors);
}

void AimBot::run_dynamic(std::vector<Armor> armors, ReadPack work_msg, ArmorDir dir, const Pose &pose, double yaw_speed)
{

    // mcu_data.enemy_number = 7; // 只识别前哨战
    onEnter();
    // TODO: what propose for canvas?
    // canvas = image;

    if (work_msg.isRightMouseButtonClicked && std::abs(last_click_right - pose.mcu_time) > 200)
    {
        // DEBUG_IP(
        //     std::cout<<"click right!"<<std::endl;
        // );
        RCLCPP_INFO(autoaim_node->get_logger(), "click right!");
        this->curr_state = AimState::SEARCH_STATE;
        last_click_right = pose.mcu_time;
        center_switch = true;
        searching = true;
    }

    if (curr_state == AimState::SEARCH_STATE)
    {
        targetSearched = searchArmor_dynatic(armors, work_msg);
    }
    else
    {
        searching = false;
        // a=false;
        trackArmor_dynamic(armors, work_msg, dir, pose, yaw_speed);
        if (flag_track_car_failed) // 在车辆都track失败的情况下重新search，几乎为进入search的唯一条件
        {
            if_next_frame_track = false;
        }
        else
        {
            if_next_frame_track = true;
        }
    }
    // DEBUG(drawArmors(canvas, armors); drawBars(canvas, lightbars); displayInfo(canvas););
    onExit();
    // curr_state =2;
}

bool AimBot::searchArmor_dynatic(std::vector<Armor> armors, ReadPack work_msg)
{
#ifndef _USE_NETWORK
    // processImage(image, mcu_data.enemy_color);            /* 图像预处理 */
    // findLightbars(processed_image, mcu_data.enemy_color); /* 寻找灯条 */
    // matchArmors(lightbars, armors, pose);                 /* 灯条两两匹配装甲板 */
#else
    detectArmor(image);
#endif
    // gradeArmors(armors);                                                 /* 对匹配得到的装甲板们进行误差分数赋值 */
    // sortArmors(armors);                                                  /* 根据误差分数降序排列 */
    trySearchArmor(armors, work_msg.enemy_number, work_msg.enemy_color); /* 数字识别过滤误识别的装甲板并排序 */
    for (auto iter = armors.begin(); iter != armors.end();)
    {
        /* 丢弃误识别装甲板 */
        bool ToDiscard = (iter->classifier_num == 7);
        if (!ToDiscard)
        {
            iter = armors.erase(iter);
        }
        else
        {
            ++iter;
        }
    }
    if (!armors.empty())
    {
        flag_switch_armor = true;
        armor_lost_cnt = 0;
        car_lost_cnt = 0;
        target_armor = armors.at(0);
        curr_number = target_armor.classifier_num;
        if (ROI_ENABLE)
        {
            setRoiRect(target_armor.rect()); // 把框出装甲板的旋转矩形的包围矩形适当放大
        }
        armorFound = true;
    }
    else
    {
        flag_switch_armor = false;
        roi_rect = cv::Rect();
        armorFound = false;
    }
    // response->point.x = roi_rect.x;
    // response->point.y = roi_rect.y;
    // response->height = roi_rect.height;
    // response->width = roi_rect.width;
    // response->flag_if_search = true;
    return armorFound;
}

void AimBot::trackArmor_dynamic(std::vector<Armor> armors, ReadPack work_msg, ArmorDir dir, const Pose &pose, double yaw_speed)
{
#ifndef _USE_NETWORK
    // processImage(image, mcu_data.enemy_color);            /* 图像预处理 */
    // findLightbars(processed_image, mcu_data.enemy_color); /* 寻找灯条 */
    // matchArmors(lightbars, armors, pose);                       /* 灯条两两匹配装甲板 */
#else
    detectArmor(image, mcu_data.enemy_color, true);
#endif
    try_track_dynamic(armors, dir, work_msg.enemy_color, yaw_speed); /* 追踪装甲板 */

    // if(armor_lost_cnt==0){
    //     begin_lost_time = pose.mcu_time;
    // }
    // if(pose.mcu_time-begin_lost_time<200&&flag_track_car_failed==true){
    //     flag_track_car_failed=false;
    // }

    if (armorFound)
    {
        if (ROI_ENABLE)
        {
            setRoiRect(target_armor.rect()); // 把框出装甲板的旋转矩形的包围矩形适当放大
        }
    }
    else
    {
        roi_rect = cv::Rect();
    }
    // response->point.x = roi_rect.x;
    // response->point.y = roi_rect.y;
    // response->height = roi_rect.height;
    // response->width = roi_rect.width;
    // response->flag_if_search = false;
}
